//
// Created by shuoy on 8/10/21.
// Modified by ycshao on Aug 17, 2022
//

#pragma once

#include <Eigen/Core>


template<typename T>
class kinematics
{

public:
    kinematics() = default;
    ~kinematics() = default;

    const int RHO_OPT_SIZE = 3;
    const int RHO_FIX_SIZE = 5;
    // rho opt are contact offset cx cy cz
    // rho fix are body offset x& y, thigh offset, upper leg length, lower leg length
    // functions with eigen interface

    // forward kinematics 3x1
    Eigen::Matrix<T, 3, 1> fk(Eigen::Matrix<T, 3, 1> q, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_opt, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_fix)
    {
        Eigen::Matrix<T, 3, 1> out;
        autoFunc_fk_derive(q.data(), rho_opt.data(), rho_fix.data(), out.data());
        return out;
    }

    // jacobian   3x3
    Eigen::Matrix<T, 3, 3> jac(Eigen::Matrix<T, 3, 1> q, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_opt, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_fix)
    {
        Eigen::Matrix<T, 3, 3> mtx;
        autoFunc_d_fk_dq(q.data(), rho_opt.data(), rho_fix.data(), mtx.data());
        return mtx;
    }

    // the partial derivative of fk wrt rho opt   3x3
    Eigen::Matrix<T, 3, 3> dfk_drho(Eigen::Matrix<T, 3, 1> q, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_opt, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_fix)
    {
        Eigen::Matrix<T, 3, 3> mtx;
        autoFunc_d_fk_dc(q.data(), rho_opt.data(), rho_fix.data(), mtx.data());
        return mtx;
    }

    // the partial derivative of jacobian wrt q    9x3
    Eigen::Matrix<T, 9, 3> dJ_dq(Eigen::Matrix<T, 3, 1> q, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_opt, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_fix)
    {
        Eigen::Matrix<T, 9, 3> mtx;
        autoFunc_dJ_dq(q.data(), rho_opt.data(), rho_fix.data(), mtx.data());
        return mtx;
    }

    // the partial derivative of jacobian wrt rho opt   9x3
    Eigen::Matrix<T, 9, 3> dJ_drho(Eigen::Matrix<T, 3, 1> q, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_opt, Eigen::Matrix<T, Eigen::Dynamic, 1> rho_fix)
    {
        Eigen::Matrix<T, 9, 3> mtx;
        autoFunc_dJ_dpho(q.data(), rho_opt.data(), rho_fix.data(), mtx.data());
        return mtx;
    }

private:
    // functions with basic C++ interface, generated by Matlab
    inline void autoFunc_fk_derive(const T in1[3], const T in2[3], const T in3[5], T p_bf[3])
    {
        T p_bf_tmp;
        T t2;
        T t3;
        T t4;
        T t5;
        T t6;
        T t7;
        T t8;
        T t9;

        //     This function was generated by the Symbolic Math Toolbox version 8.6.
        //     10-Aug-2021 14:48:21
        t2 = std::cos(in1[0]);
        t3 = std::cos(in1[1]);
        t4 = std::cos(in1[2]);
        t5 = std::sin(in1[0]);
        t6 = std::sin(in1[1]);
        t7 = std::sin(in1[2]);
        t8 = in1[1] + in1[2];
        t9 = std::sin(t8);
        p_bf[0] = (((in3[0] + in2[2] * t9) - in3[4] * t9) - t6 * in3[3]) + in2[0] *
                                                                               std::cos(t8);
        p_bf[1] = ((((((((in3[1] + in2[1] * t2) + in3[2] * t2) + t3 * t5 * in3[3]) +
                       in2[0] * t3 * t5 * t7) +
                      in2[0] * t4 * t5 * t6) -
                     in2[2] * t3 *
                         t4 * t5) +
                    in2[2] * t5 * t6 * t7) +
                   in3[4] * t3 * t4 * t5) -
                  in3
                          [4] *
                      t5 * t6 * t7;
        t8 = in2[0] * t2;
        t9 = in2[2] * t2;
        p_bf_tmp = in3[4] * t2;
        p_bf[2] = (((((((in2[1] * t5 + in3[2] * t5) - t2 * t3 * in3[3]) - t8 * t3 * t7) - t8 * t4 * t6) + t9 * t3 * t4) - t9 * t6 * t7) - p_bf_tmp * t3 *
                                                                                                                                              t4) +
                  p_bf_tmp * t6 * t7;
    }

    inline void autoFunc_d_fk_dq(const T in1[3], const T in2[3], const T in3[5], T jacobian[9])
    {
        T b_jacobian_tmp;
        T c_jacobian_tmp;
        T jacobian_tmp;
        T t12;
        T t16;
        T t17;
        T t2;
        T t22;
        T t3;
        T t4;
        T t5;
        T t6;
        T t7;
        T t8;
        T t9;

        //     This function was generated by the Symbolic Math Toolbox version 8.6.
        //     10-Aug-2021 14:48:21
        t2 = std::cos(in1[0]);
        t3 = std::cos(in1[1]);
        t4 = std::cos(in1[2]);
        t5 = std::sin(in1[0]);
        t6 = std::sin(in1[1]);
        t7 = std::sin(in1[2]);
        t8 = in1[1] + in1[2];
        t9 = std::cos(t8);
        t8 = std::sin(t8);
        t12 = in2[0] * t9;
        t16 = in2[2] * t8;
        t17 = in3[4] * t8;
        t22 = (t12 + t16) + -t17;
        jacobian[0] = 0.0;
        jacobian_tmp = in2[0] * t2;
        b_jacobian_tmp = in2[2] * t2;
        c_jacobian_tmp = in3[4] * t2;
        jacobian[1] = (((((((-in2[1] * t5 - in3[2] * t5) + t2 * t3 * in3[3]) +
                           jacobian_tmp * t3 * t7) +
                          jacobian_tmp * t4 * t6) -
                         b_jacobian_tmp * t3 * t4) +
                        b_jacobian_tmp * t6 * t7) +
                       c_jacobian_tmp * t3 * t4) -
                      c_jacobian_tmp * t6 * t7;
        jacobian[2] = (((((((in2[1] * t2 + in3[2] * t2) + t3 * t5 * in3[3]) + in2[0] *
                                                                                  t3 * t5 * t7) +
                          in2[0] * t4 * t5 * t6) -
                         in2[2] * t3 * t4 *
                             t5) +
                        in2[2] * t5 * t6 * t7) +
                       in3[4] * t3 * t4 * t5) -
                      in3[4] * t5 * t6 * t7;
        jacobian_tmp = (in2[2] * t9 + -(in3[4] * t9)) + -(in2[0] * t8);
        jacobian[3] = jacobian_tmp - t3 * in3[3];
        b_jacobian_tmp = ((t6 * in3[3] - t12) - t16) + t17;
        jacobian[4] = -t5 * b_jacobian_tmp;
        jacobian[5] = t2 * b_jacobian_tmp;
        jacobian[6] = jacobian_tmp;
        jacobian[7] = t5 * t22;
        jacobian[8] = -t2 * t22;
    }

    inline void autoFunc_d_fk_dc(const T in1[3], const T in2[3], const T in3[5], T d_fk_dc[9])
    {
        T t2;
        T t3;
        T t4;
        T t5;

        //     This function was generated by the Symbolic Math Toolbox version 8.6.
        //     10-Aug-2021 14:48:21
        t2 = std::cos(in1[0]);
        t3 = std::sin(in1[0]);
        t4 = in1[1] + in1[2];
        t5 = std::cos(t4);
        t4 = std::sin(t4);
        d_fk_dc[0] = t5;
        d_fk_dc[1] = t3 * t4;
        d_fk_dc[2] = -t2 * t4;
        d_fk_dc[3] = 0.0;
        d_fk_dc[4] = t2;
        d_fk_dc[5] = t3;
        d_fk_dc[6] = t4;
        d_fk_dc[7] = -t3 * t5;
        d_fk_dc[8] = t2 * t5;
    }

    inline void autoFunc_dJ_dq(const T in1[3], const T in2[3], const T in3[5], T dJ_dq[27])
    {
        T t10;
        T t13;
        T t17;
        T t18;
        T t2;
        T t26;
        T t27;
        T t28;
        T t3;
        T t31;
        T t34;
        T t34_tmp;
        T t35;
        T t4;
        T t5;
        T t6;
        T t7;
        T t8;
        T t9;

        //     This function was generated by the Symbolic Math Toolbox version 8.6.
        //     10-Aug-2021 14:48:21
        t2 = std::cos(in1[0]);
        t3 = std::cos(in1[1]);
        t4 = std::cos(in1[2]);
        t5 = std::sin(in1[0]);
        t6 = std::sin(in1[1]);
        t7 = std::sin(in1[2]);
        t8 = in1[1] + in1[2];
        t9 = std::cos(t8);
        t10 = t3 * in3[3];
        t8 = std::sin(t8);
        t13 = in2[0] * t9;
        t17 = in2[2] * t8;
        t18 = in3[4] * t8;
        t9 = (in3[4] * t9 + in2[0] * t8) + -(in2[2] * t9);
        t8 = (t13 + t17) + -t18;
        t26 = (t18 + -t13) + -t17;
        t27 = t5 * t8;
        t28 = t2 * t9;
        t18 = t2 * t8;
        t31 = t10 + t9;
        t34_tmp = t6 * in3[3] + t26;
        t34 = -t2 * t34_tmp;
        t35 = -t5 * t34_tmp;
        t8 = -(t5 * t9);
        dJ_dq[0] = 0.0;
        dJ_dq[1] = (((((((-in2[1] * t2 - in3[2] * t2) - t5 * t10) - in2[0] * t3 * t5 *
                                                                        t7) -
                       in2[0] * t4 * t5 * t6) +
                      in2[2] * t3 * t4 * t5) -
                     in2[2] * t5 * t6 * t7) -
                    in3[4] * t3 * t4 * t5) +
                   in3[4] * t5 * t6 * t7;
        t9 = in2[0] * t2;
        t13 = in2[2] * t2;
        t17 = in3[4] * t2;
        dJ_dq[2] = (((((((-in2[1] * t5 - in3[2] * t5) + t2 * t10) + t9 * t3 * t7) + t9 * t4 * t6) - t13 * t3 * t4) + t13 * t6 * t7) + t17 * t3 * t4) -
                   t17 * t6 * t7;
        dJ_dq[3] = 0.0;
        dJ_dq[4] = t34;
        dJ_dq[5] = t35;
        dJ_dq[6] = 0.0;
        dJ_dq[7] = t18;
        dJ_dq[8] = t27;
        dJ_dq[9] = 0.0;
        dJ_dq[10] = t34;
        dJ_dq[11] = t35;
        dJ_dq[12] = t34_tmp;
        dJ_dq[13] = -t5 * t31;
        dJ_dq[14] = t2 * t31;
        dJ_dq[15] = t26;
        dJ_dq[16] = t8;
        dJ_dq[17] = t28;
        dJ_dq[18] = 0.0;
        dJ_dq[19] = t18;
        dJ_dq[20] = t27;
        dJ_dq[21] = t26;
        dJ_dq[22] = t8;
        dJ_dq[23] = t28;
        dJ_dq[24] = t26;
        dJ_dq[25] = t8;
        dJ_dq[26] = t28;
    }

    inline void autoFunc_dJ_dpho(const T in1[3], const T[3], const T[5], T dJ_dpho[27])
    {
        T t11;
        T t12;
        T t13;
        T t2;
        T t3;
        T t4;
        T t5;
        T t9;

        //     This function was generated by the Symbolic Math Toolbox version 8.6.
        //     10-Aug-2021 14:48:22
        t2 = std::cos(in1[0]);
        t3 = std::sin(in1[0]);
        t4 = in1[1] + in1[2];
        t5 = std::cos(t4);
        t4 = std::sin(t4);
        t9 = t3 * t5;
        t11 = t3 * t4;
        t12 = -(t2 * t5);
        t13 = t2 * -t4;
        dJ_dpho[0] = 0.0;
        dJ_dpho[1] = t2 * t4;
        dJ_dpho[2] = t11;
        dJ_dpho[3] = -t4;
        dJ_dpho[4] = t9;
        dJ_dpho[5] = t12;
        dJ_dpho[6] = -t4;
        dJ_dpho[7] = t9;
        dJ_dpho[8] = t12;
        dJ_dpho[9] = 0.0;
        dJ_dpho[10] = -t3;
        dJ_dpho[11] = t2;
        dJ_dpho[12] = 0.0;
        dJ_dpho[13] = 0.0;
        dJ_dpho[14] = 0.0;
        dJ_dpho[15] = 0.0;
        dJ_dpho[16] = 0.0;
        dJ_dpho[17] = 0.0;
        dJ_dpho[18] = 0.0;
        dJ_dpho[19] = t12;
        dJ_dpho[20] = -t9;
        dJ_dpho[21] = t5;
        dJ_dpho[22] = t11;
        dJ_dpho[23] = t13;
        dJ_dpho[24] = t5;
        dJ_dpho[25] = t11;
        dJ_dpho[26] = t13;
    }
};
